
#include <math.h>

#include "../../geometry/geometry.h"
#include "../../geometry/delaunay.h"

#include "../../matrix/matrix.h"
#include "../../algebra/algebra.h"

#include "../../algebra/alg_primitives.h"
#include "../../stat/stat_primitives.h"

#include "../features/xcorner.h"
#include "../pointop/threshold.h"
#include "chessboard.h"

#include "../image.h"
#include "../io/imageio.h"
#include "../draw/draw.h"

boolean imp_image_calib_find_chessboard(ImpMatrix * Mat, ImpMatrix ** Xr, ImpMatrix ** Xi, int w, int h) {

	int nxcorner;

	ImpMatrix * Corners = NULL;
	ImpMatrix * NMSi = NULL;
	ImpMatrix * NMS = NULL;
	ImpMatrix * Label = NULL;
	ImpMatrix * Feat = NULL;
	ImpMatrix * BW = NULL;
	ImpSurface * surf = NULL;
	ChessGrid * cgrid = NULL;

	nxcorner = imp_image_features_Xcorner(Mat, &Corners);

	if (nxcorner < (w + h)) {
		imp_object_delete(Corners);
		return FALSE;
	}

	imp_image_nonmaxsup_feature_nonzero(Corners, 3, &NMSi);
	imp_image_threshold(NMSi, 0, 0, 255, IMP_THRESHOLD_BINARY, NULL);
	imp_image_morph_dilate_w(NMSi, 3, &NMS);

	int n;
	n = imp_image_label(NMS, &Label, 255.0);
	imp_image_label_analisys(Label, n, &Feat);

	imp_matrix_clear(NMS);
	for (n = 0; n < Feat->rows; n++) {
		int c = Feat->data[ n * Feat->columns + 8 ];
		int r = Feat->data[ n * Feat->columns + 9 ];

		NMS->data[ r * NMS->columns + c ] = 255;
	}

	surf = imp_delaunay_from_matrix(NMS, TRUE);

	imp_image_threshold_adaptive(Mat, IMP_THRESHOLD_ADAPTIVE_WELLNER_EXTENDED, &BW);

	filt_chessboard(surf, BW);

	imp_surface_remove_orfans_edges(surf);
	imp_surface_remove_orfans_vertices(surf);

	if (surf->vertices->size > (w * h)) {
		// This should never happen

		ImpImage * RGB = NULL;
		ImpDraw draw;

		imp_image_mat2rgb(Mat, &RGB);
		draw = imp_draw_create(RGB);
		draw.fg = imp_color(255, 0, 0);
		imp_draw_list_of_geom_triangles(&draw, surf->faces, TRUE);

		imp_image_save_matrix_as_image(Mat, TRUE, "00-mat.pgm");
		imp_image_save_matrix_as_image(BW, TRUE, "01-bw.pgm");
		imp_image_save_matrix_as_image(NMSi, TRUE, "02-nms.pgm");
		imp_image_save_matrix_as_image(Label, TRUE, "03-labels.pgm");
		imp_image_save_matrix_as_image(NMS, TRUE, "04-nms_final.pgm");
	}
	imp_object_delete(NMSi);
	imp_object_delete(Label);
	imp_object_delete(Feat);


	if (surf->vertices->size == 0) {

		imp_object_delete(Corners);
		imp_object_delete(NMS);
		imp_object_delete(BW);
		imp_surface_remove_all_elements_full(surf);
		imp_object_delete(surf);

		return FALSE;
	}


	cgrid = create_chessboard_correspondence(surf, BW);

	/*
		if (cgrid->size < min || cgrid->size > max) {

			imp_object_delete(Corners);
			imp_object_delete(NMS);
			imp_object_delete(BW);
			imp_surface_remove_all_elements_full(surf);
			imp_object_delete(surf);
			mem_free(cgrid->points);
			mem_free(cgrid);

			return FALSE;
		}
	 */

	extract_matrices_from_chessgrid(cgrid, Xr, Xi);

	//update_points_to_sub_pixel(Mat, *Xi);

	imp_object_delete(Corners);
	imp_object_delete(NMS);
	imp_object_delete(BW);
	imp_surface_remove_all_elements_full(surf);
	imp_object_delete(surf);
	mem_free(cgrid->points);
	mem_free(cgrid);

	return TRUE;
}

boolean imp_image_calib_find_partial_chessboard(ImpMatrix * Mat, ImpMatrix ** Xr, ImpMatrix ** Xi) {

	int nxcorner;

	ImpMatrix * Corners = NULL;
	ImpMatrix * NMSi = NULL;
	ImpMatrix * NMS = NULL;
	ImpMatrix * Label = NULL;
	ImpMatrix * Feat = NULL;
	ImpMatrix * BW = NULL;
	ImpSurface * surf = NULL;
	ChessGrid * cgrid = NULL;

	nxcorner = imp_image_features_Xcorner(Mat, &Corners);
	//nxcorner = imp_image_features_XcornerN(Mat, &Corners, 2);

	if (nxcorner < 4) {
		imp_object_delete(Corners);
		return FALSE;
	}

	imp_image_nonmaxsup_feature_nonzero(Corners, 3, &NMSi);
	imp_image_threshold(NMSi, 0, 0, 255, IMP_THRESHOLD_BINARY, NULL);
	imp_image_morph_dilate_w(NMSi, 3, &NMS);


	int n;
	n = imp_image_label(NMS, &Label, 255.0);
	imp_image_label_analisys(Label, n, &Feat);

	imp_matrix_clear(NMS);
	for (n = 0; n < Feat->rows; n++) {
		int c = Feat->data[ n * Feat->columns + 8 ];
		int r = Feat->data[ n * Feat->columns + 9 ];

		NMS->data[ r * NMS->columns + c ] = 255;
	}

	surf = imp_delaunay_from_matrix(NMS, TRUE);

	imp_image_threshold_adaptive(Mat, IMP_THRESHOLD_ADAPTIVE_WELLNER_EXTENDED, &BW);

	filt_chessboard(surf, BW);

	imp_surface_remove_orfans_edges(surf);
	imp_surface_remove_orfans_vertices(surf);

	//if( surf->vertices->size <= 0 ) {
	//	surf->vertices->size = 0;
	//}

	if (surf->vertices->size > 77) {
		// This should never happen

		ImpImage * RGB = NULL;
		ImpDraw draw;

		imp_image_mat2rgb(Mat, &RGB);
		draw = imp_draw_create(RGB);
		draw.fg = imp_color(255, 0, 0);
		imp_draw_list_of_geom_triangles(&draw, surf->faces, TRUE);

		imp_image_save_matrix_as_image(Mat, TRUE, "00-mat.pgm");
		imp_image_save_matrix_as_image(BW, TRUE, "01-bw.pgm");
		imp_image_save_matrix_as_image(NMSi, TRUE, "02-nms.pgm");
		imp_image_save_matrix_as_image(Label, TRUE, "03-labels.pgm");
		imp_image_save_matrix_as_image(NMS, TRUE, "04-nms_final.pgm");
	}
	imp_object_delete(NMSi);
	imp_object_delete(Label);
	imp_object_delete(Feat);


	if (surf->vertices->size == 0) {

		imp_object_delete(Corners);
		imp_object_delete(NMS);
		imp_object_delete(BW);
		imp_surface_remove_all_elements_full(surf);
		imp_object_delete(surf);

		return FALSE;
	}

	cgrid = create_chessboard_correspondence(surf, BW);

	/*
		if (cgrid->size < min || cgrid->size > max) {

			imp_object_delete(Corners);
			imp_object_delete(NMS);
			imp_object_delete(BW);
			imp_surface_remove_all_elements_full(surf);
			imp_object_delete(surf);
			mem_free(cgrid->points);
			mem_free(cgrid);

			return FALSE;
		}
	 */

	extract_matrices_from_chessgrid(cgrid, Xr, Xi);

	//get_search_grid(*Xr, 11, 7);

	update_points_to_sub_pixel(Mat, *Xi);

	imp_object_delete(Corners);
	imp_object_delete(NMS);
	imp_object_delete(BW);
	imp_surface_remove_all_elements_full(surf);
	imp_object_delete(surf);
	mem_free(cgrid->points);
	mem_free(cgrid);

	return TRUE;
}

/**
 * 
 * @param A - input data. A is of type: A = [ p1 p2 p3 p4 p5 ... pn ]
 * @param An - normalized data
 * @param T - Matrix Transformation
 */
static void __normalize_points(ImpMatrix * A, ImpMatrix ** An, ImpMatrix ** T) {

	if (*An == NULL) (*An) = imp_matrix_new(A->rows, A->columns);
	if (*T == NULL) (*T) = imp_matrix_new(3, 3);

	int m = A->rows;
	int n = A->columns;

	real * x = A->data;
	real xc[A->size];
	real * xn = (*An)->data;

	real mean[2];
	real length[n];
	real ulen;
	real s;

	__mmean_row(m, n, &x[0], n, &mean[0], 1);

	int i;
	for (i = 0; i < m; i++) {
		__vinc(-mean[i], n, &x[i * n], 1, &xc[i * n], 1);
	}

	__mpow(m, n, &xc[0], n, 2, &xn[0], n);

	__msum(m, n, &xn[0], n, length, 1);
	__vpow(0.5, n, length, 1, length, 1);

	ulen = __vmean(n, length, 1);

	s = sqrt(2) / ulen;

	__mscale(m, n, xc, n, s, xn, n);

	(*T)->data[0] = s;
	(*T)->data[1] = 0;
	(*T)->data[2] = -s * mean[0];

	(*T)->data[3] = 0;
	(*T)->data[4] = s;
	(*T)->data[5] = -s * mean[1];

	(*T)->data[6] = 0;
	(*T)->data[7] = 0;
	(*T)->data[8] = 1;

}

static void __make_system_for_homography(ImpMatrix * Xw, ImpMatrix * Xi, ImpMatrix ** M) {

	int rows;
	int columns;

	real xr, yr;
	real xp, yp;

	rows = Xw->rows;
	columns = Xw->columns;

	int r;
	for (r = 0; r < columns; r++) {

		xr = Xw->data[ r ];
		yr = Xw->data[ r + columns ];

		xp = Xi->data[ r ];
		yp = Xi->data[ r + columns ];

		int ra1 = (2 * r) * (*M)->columns;
		int ra2 = (2 * r + 1) * (*M)->columns;

		// =====================================================================
		(*M)->data[ 0 + ra1 ] = 0;
		(*M)->data[ 1 + ra1 ] = 0;
		(*M)->data[ 2 + ra1 ] = 0;

		(*M)->data[ 3 + ra1 ] = -1 * xr;
		(*M)->data[ 4 + ra1 ] = -1 * yr;
		(*M)->data[ 5 + ra1 ] = -1 * 1;

		(*M)->data[ 6 + ra1 ] = yp * xr;
		(*M)->data[ 7 + ra1 ] = yp * yr;
		(*M)->data[ 8 + ra1 ] = yp * 1;

		// =====================================================================
		(*M)->data[ 0 + ra2 ] = 1 * xr;
		(*M)->data[ 1 + ra2 ] = 1 * yr;
		(*M)->data[ 2 + ra2 ] = 1 * 1;

		(*M)->data[ 3 + ra2 ] = 0;
		(*M)->data[ 4 + ra2 ] = 0;
		(*M)->data[ 5 + ra2 ] = 0;

		(*M)->data[ 6 + ra2 ] = -xp * xr;
		(*M)->data[ 7 + ra2 ] = -xp * yr;
		(*M)->data[ 8 + ra2 ] = -xp * 1;
	}
}

void nDLT(ImpMatrix * Xw, ImpMatrix * Xi, ImpMatrix ** H) {

	ImpMatrix * T = imp_matrix_new(3, 3);
	ImpMatrix * Ti = imp_matrix_new(3, 3);
	ImpMatrix * Ti_inv = imp_matrix_new(3, 3);

	ImpMatrix * Xn = imp_matrix_new(Xw->rows, Xw->columns);
	ImpMatrix * Xin = imp_matrix_new(Xi->rows, Xi->columns);

	ImpMatrix * Hn = imp_matrix_new(3, 3);

	ImpMatrix * A = imp_matrix_new(Xw->columns * 2, 9);

	// = Point normalization ===================================================
	__normalize_points(Xw, &Xn, &T);
	__normalize_points(Xi, &Xin, &Ti);

	// = Linear Equation System ================================================
	__make_system_for_homography(Xn, Xin, &A);

	// = Solve Linear System ===================================================
	int m = A->rows;
	int n = A->columns;

	ImpMatrix * U = imp_matrix_new(m, m);
	ImpMatrix * S = imp_matrix_new(n, 1);
	ImpMatrix * V = imp_matrix_new(n, n);

	svd(m, n, A->data, S->data, U->data, V->data);

	// = Get the normalized Homography =========================================
	int i;
	double s;
	s = S->data[0];
	n = 0;
	for (i = 1; i < S->size; i++) {
		if (S->data[i] < s) {
			s = S->data[i];
			n = i;
		}
	}
	for (i = 0; i < A->columns; i++) {
		Hn->data[i] = V->data[ i * V->columns + n ];
	}

	// = De-normalization of Homography ========================================
	imp_algebra_inv_GaussJordan(Ti, &Ti_inv);

	imp_algebra_mult(Ti_inv, Hn, &Ti);

	if (*H == NULL) (*H) = imp_matrix_new(3, 3);

	imp_algebra_mult(Ti, T, H);

	real norm = __vnorm(9, (*H)->data, 1);

	for (i = 0; i < 9; i++) {
		(*H)->data[i] /= norm;
	}

	imp_object_delete(T);
	imp_object_delete(Ti_inv);
	imp_object_delete(Ti);
	imp_object_delete(Xn);
	imp_object_delete(Xin);
	imp_object_delete(Hn);
	imp_object_delete(A);

	imp_object_delete(U);
	imp_object_delete(S);
	imp_object_delete(V);

}

ImpMatrix * getInversePerspective(ImpMatrix * H) {
	ImpMatrix * Hi = NULL;
	imp_algebra_inv_GaussJordan(H, &Hi);
	return Hi;
}

void undist_image(ImpMatrix * Mat, ImpMatrix * H, ImpMatrix ** Out) {

	real * h = H->data;

	//imp_matrix_print(H);


	int rows = Mat->rows;
	int columns = Mat->columns;

	if (*Out == NULL) (*Out) = imp_matrix_new(rows, columns);

	int x, y, z = 1;
	real xi, yi, zi;
	real val;
	for (y = 0; y < rows; y++) {
		for (x = 0; x < columns; x++) {

			int i = x + y*columns;

			int _x = (x * 10);
			int _y = (y * 10);

			val = Mat->data[ i ];


			xi = h[0] * _x + h[1] * _y + h[2] * z;
			yi = h[3] * _x + h[4] * _y + h[5] * z;
			zi = h[6] * _x + h[7] * _y + h[8] * z;

			xi = xi / zi;
			yi = yi / zi;

			xi += 150;
			yi += 150;

			if (xi >= columns || yi >= rows) continue;
			if (xi < 0 || yi < 0) continue;



			i = (int) xi + (int) yi*columns;
			(*Out)->data[ i ] = val;
		}
	}


}


